package com.canbot.u05.control;

/**
 * Created by Administrator on 2017/5/6.
 */

public class EncodeUtil {

        private static byte msgID = 0;

        /**
         * 计算校验和
         *
         * @param data
         * @return
         */
        private static byte check(byte[] data) {
                byte check = 0;
                for (int i = 8, len = data.length - 1; i < len; i++) {
                        check ^= data[i];
                }
                return check;
        }

        /**
         * 手臂运动控制 --- 动作库里面的动作的执行
         *
         * @return
         */
        public static byte[] amrSportControl(int actionID) {
                int len = 6;// 数据长度是 6 个字节
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_armSportControl & 0xff;
                buf[index++] = Cmd.Num_armSportControl >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);
                buf[index++] = 1;
                buf[index++] = (byte) (actionID & 0xff);
                buf[index++] = (byte) (actionID >> 8);
                buf[index++] = 0;
                buf[index++] = msgID++;
                buf[index++] = check(buf);
                return buf;
        }

        /**
         * @param cmd            运动方式：0-使能底盘 1-自由运动  2-固定距离运动 3-固定角度运动 4-固定时间运动 5-停止
         * @param moveSpeed      前进速度
         * @param rotateSpeed    旋转速度
         * @param direction      运动方向
         * @param moveDistance   前进距离
         * @param rotateDistance 旋转距离
         * @param time           运动时间
         * @return
         */
        public static byte[] directoinSportControl(byte cmd, short moveSpeed, short rotateSpeed, byte direction, short moveDistance, short rotateDistance, short time) {
                int len = 14;// 数据长度是 6 个字节
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_directionSportControl & 0xff;
                buf[index++] = Cmd.Num_directionSportControl >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);

                buf[index++] = cmd;
                buf[index++] = (byte) (moveSpeed & 0xff);
                buf[index++] = (byte) (moveSpeed >> 8);

                buf[index++] = (byte) (rotateSpeed & 0xff);
                buf[index++] = (byte) (rotateSpeed >> 8);

                buf[index++] = direction;
                buf[index++] = (byte) (moveDistance & 0xff);
                buf[index++] = (byte) (moveDistance >> 8);

                buf[index++] = (byte) (rotateDistance & 0xff);
                buf[index++] = (byte) (rotateDistance >> 8);

                buf[index++] = (byte) (time & 0xff);
                buf[index++] = (byte) (time >> 8);

                buf[index++] = msgID++;
                buf[index++] = check(buf);
                return buf;
        }

        /**
         * 地图控制相关
         *
         * @param cmd 0-打开  1-关闭 2-查询
         * @return
         */
        public static byte[] mapSwitch(byte cmd) {
                int len = 2;// 数据长度是 6 个字节
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_mapSwitch & 0xff;
                buf[index++] = Cmd.Num_mapSwitch >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);
                buf[index++] = cmd;
                buf[index++] = check(buf);
                return buf;
        }

        /**
         * 运动自动测试相关
         *
         * @param cmd 1：开始测试   2：停止测试  3：清空结果
         * @return
         */
        public static byte[] sportAutoTest(byte cmd) {
                int len = 1;// 数据长度是 6 个字节
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_mapSwitch & 0xff;
                buf[index++] = Cmd.Num_mapSwitch >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);
                buf[index++] = cmd;
                return buf;
        }

        /**
         * 发送键盘控制
         *
         * @return
         */
        public static byte[] keyControl(int key) {
                short speed = 300;
                int len = 1;
                if (key == Cmd.Key_houtui || key == Cmd.Key_qianjin) {
                        len = 3;
                }
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_key & 0xff;
                buf[index++] = Cmd.Num_key >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);
                buf[index++] = (byte) key;
                if (key == Cmd.Key_houtui || key == Cmd.Key_qianjin) {
                        buf[index++] = (byte) (speed & 0xff);
                        buf[index++] = (byte) (speed >> 8);
                }
                return buf;
        }

        /**
         * 关节控制
         *
         * @return
         */
        public static byte[] jointControl(int joint, short angle) {
                short speed = 0;
                short time = 1000;
                int len = 21;
                int commandCode = 1;//控制位置
                byte[] buf = new byte[8 + len];
                int index = 0;
                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_jointControl & 0xff;
                buf[index++] = Cmd.Num_jointControl >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);

                buf[index++] = (byte) (commandCode & 0xff);
                buf[index++] = (byte) (commandCode >> 8);
                buf[index++] = (byte) (commandCode >> 16);
                buf[index++] = (byte) (commandCode >> 24);

                buf[index++] = (byte) (joint & 0xff);
                buf[index++] = (byte) (joint >> 8);
                buf[index++] = (byte) (joint >> 16);
                buf[index++] = (byte) (joint >> 24);

                buf[index++] = (byte) (angle & 0xff);
                buf[index++] = (byte) (angle >> 8);
                buf[index++] = (byte) (angle >> 16);
                buf[index++] = (byte) (angle >> 24);

                buf[index++] = (byte) (speed & 0xff);
                buf[index++] = (byte) (speed >> 8);
                buf[index++] = (byte) (speed >> 16);
                buf[index++] = (byte) (speed >> 24);

                buf[index++] = (byte) (time & 0xff);
                buf[index++] = (byte) (time >> 8);
                buf[index++] = (byte) (time >> 16);
                buf[index++] = (byte) (time >> 24);

                buf[index++] = check(buf);
                return buf;
        }

        /**
         * 开放运动控制
         *
         * @return
         */
        public static byte[] openSportControl(short xSpeed, short ySpeed, short rotateSpeed) {
                int len = 13;
                byte[] buf = new byte[8 + len];
                int index = 0;
                byte directionMove = 0;
                int moveTime = 1000;

                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_openSportControl & 0xff;
                buf[index++] = Cmd.Num_openSportControl >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);

                buf[index++] = directionMove;  // 底盘是否动作

                buf[index++] = (byte) (xSpeed & 0xff);
                buf[index++] = (byte) (xSpeed >> 8);
                buf[index++] = (byte) (ySpeed & 0xff);
                buf[index++] = (byte) (ySpeed >> 8);
                buf[index++] = (byte) (rotateSpeed & 0xff);
                buf[index++] = (byte) (rotateSpeed >> 8);

                buf[index++] = (byte) (moveTime & 0xff); // 底盘运动时间
                buf[index++] = (byte) (moveTime >> 8);
                buf[index++] = (byte) (moveTime >> 16);
                buf[index++] = (byte) (moveTime >> 24);

                buf[index++] = 0;  //手臂是否运动

                buf[index++] = check(buf);
                return buf;
        }

        /**
         * 发送透传数据
         *
         * @return
         */
        public static byte[] sendTouChuan(int msgType, String data) {
                int len = 2 + data.getBytes().length + 1;// msgType=2byte  + data + 检验位1byte
                byte[] buf = new byte[8 + len];
                int index = 0;
                byte directionMove = 0;
                int moveTime = 1000;

                buf[index++] = Cmd.Head_H;
                buf[index++] = Cmd.Head_L;
                buf[index++] = Cmd.Num_sendTouChuan & 0xff;
                buf[index++] = Cmd.Num_sendTouChuan >> 8;
                buf[index++] = (byte) (len & 0xff);
                buf[index++] = (byte) (len >> 8);
                buf[index++] = (byte) (len >> 16);
                buf[index++] = (byte) (len >> 24);

                //低8位
                buf[index++] = (byte) (msgType & 0xff);
                //高8位
                buf[index++] = (byte) (msgType >> 8);


                byte[] temp = data.getBytes();
                for (int i = 0, j = temp.length; i < j; i++) {
                        buf[index++] = temp[i];
                }

                buf[index++] = check(buf);
                return buf;
        }
}
